//
// Created by craipy on 23-2-7.
//

#ifndef KALMAN_LINEARMEASUREMENTMODEL_HPP
#define KALMAN_LINEARMEASUREMENTMODEL_HPP

#include "MeasurementModel.hpp"

namespace Kalman {
    template<class StateType>
    class LinearKalmanFilter;

    template<class StateType, class MeasurementType>
    class LinearMeasurementModel : public MeasurementModel<StateType, MeasurementType, StandardBase> {
        friend class LinearKalmanFilter<StateType>;

    protected:
        Eigen::Matrix<typename MeasurementType::Scalar, MeasurementType::RowsAtCompileTime, StateType::RowsAtCompileTime> H;
        using StandardBase<MeasurementType>::P;

    public:
        LinearMeasurementModel() {
            P.setIdentity();
            H.setZero();
        }

        MeasurementType h(const StateType &x) const override {
            return H * x;
        };
    };

} // Kalman

#endif //KALMAN_LINEARMEASUREMENTMODEL_HPP
